#include "mapping/2d/grid_2d.h"
#include "mapping/probability_values.h"

namespace cartographer {
namespace mapping {

Grid2D::Grid2D(const MapLimits &limits, float min_correspondence_cost, float max_correspondence_cost)
    : limits_(limits)
    , correspondence_cost_cells_(limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
                                 kUnknownCorrespondenceValue)
    , min_correspondence_cost_(min_correspondence_cost)
    , max_correspondence_cost_(max_correspondence_cost) {}

Grid2D::Grid2D(const Grid2D &grid) : limits_(grid.limits_) {
    correspondence_cost_cells_ = grid.correspondence_cost_cells_;
    min_correspondence_cost_   = grid.min_correspondence_cost_;
    max_correspondence_cost_   = grid.max_correspondence_cost_;
    update_indices_            = grid.update_indices_;
    known_cells_box_           = grid.known_cells_box_;
}

// Finishes the update sequence.
void Grid2D::FinishUpdate() {
    std::unique_lock<mutex_t> lck(cells_mutex_);
    while (!update_indices_.empty()) {
        correspondence_cost_cells_[update_indices_.back()] -= kUpdateMarker;
        update_indices_.pop_back();
    }
}

// Returns the correspondence cost of the cell with 'cell_index'.
float Grid2D::GetCorrespondenceCost(const Eigen::Array2i &cell_index) const {
    if (!limits().Contains(cell_index))
        return kMaxCorrespondenceCost;
    return ValueToCorrespondenceCost(correspondence_cost_cells()[ToFlatIndex(cell_index)]);
}

// Returns true if the correspondence cost at the specified index is known.
bool Grid2D::IsKnown(const Eigen::Array2i &cell_index) const {
    if (IsOutOfBounds(cell_index))
        return false;

    std::unique_lock<mutex_t> lck(cells_mutex_);
    size_t                    flat_index = ToFlatIndex(cell_index);
    return correspondence_cost_cells_[flat_index] != kUnknownCorrespondenceValue;
}

bool Grid2D::IsOutOfBounds(const Eigen::Array2i &cell_index) const {
    std::unique_lock<mutex_t> lck(cells_mutex_);
    size_t                    flat_index = ToFlatIndex(cell_index);
    size_t                    cells_size = correspondence_cost_cells_.size();
    return flat_index >= cells_size || !limits_.Contains(cell_index);
}

// Fills in 'offset' and 'limits' to define a subregion of that contains all known cells.
void Grid2D::ComputeCroppedLimits(Eigen::Array2i *const offset, CellLimits *const limits) const {
    std::unique_lock<mutex_t> lck(cells_mutex_);
    if (known_cells_box_.isEmpty()) {
        *offset = Eigen::Array2i::Zero();
        *limits = CellLimits(1, 1);
        return;
    }
    *offset = known_cells_box_.min().array();
    *limits = CellLimits(known_cells_box_.sizes().x() + 1, known_cells_box_.sizes().y() + 1);
}

// Grows the map as necessary to include 'point'. This changes the meaning of
// these coordinates going forward. This method must be called immediately
// after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
void Grid2D::GrowLimits(const Eigen::Vector2f &point) {
    std::unique_lock<mutex_t> lck(cells_mutex_);
    while (!limits_.Contains(limits_.GetCellIndex(point))) {
        const int grow_size   = 200;                 // 每次扩展的栅格数量
        int       x_grow_size = 0, y_grow_size = 0;  // x, y方向需要扩展的栅格数量
        int       x_offset = 0, y_offset = 0;        // limits_.max()的偏移量

        // 根据point判断地图需要往哪个方向扩展, 共有4种扩展情况
        // 向右和向下扩展时, limits_.max()不改变, 不用加上offset
        // 向上和向左扩展时, limits_.max()发生改变, 需要加上offset
        Eigen::Array2i cell = limits_.GetCellIndex(point);
        if (cell.x() >= limits_.cell_limits().num_x_cells) {  // 向右扩展
            x_grow_size = grow_size;
        }
        if (cell.y() >= limits_.cell_limits().num_y_cells) {  // 向下扩展
            y_grow_size = grow_size;
        }
        if (cell.x() <= 0) {  // 向左扩展
            x_offset    = grow_size;
            x_grow_size = grow_size;
        }
        if (cell.y() <= 0) {  // 向右扩展
            y_offset    = grow_size;
            y_grow_size = grow_size;
        }

        const MapLimits new_limits(limits_.resolution(),
                                   limits_.max() + limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
                                   CellLimits(limits_.cell_limits().num_x_cells + x_grow_size,
                                              limits_.cell_limits().num_y_cells + y_grow_size));

        const int stride   = new_limits.cell_limits().num_x_cells;
        const int offset   = x_offset + stride * y_offset;
        const int new_size = new_limits.cell_limits().num_x_cells * new_limits.cell_limits().num_y_cells;

        std::vector<uint16_t> new_cost_cells(new_size, kUnknownCorrespondenceValue);

        for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
            for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
                new_cost_cells[offset + j + i * stride] =
                    correspondence_cost_cells_[j + i * limits_.cell_limits().num_x_cells];
            }
        }
        correspondence_cost_cells_ = new_cost_cells;
        limits_                    = new_limits;
        if (!known_cells_box_.isEmpty()) {
            known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
        }
    }
}

int Grid2D::ToFlatIndex(const Eigen::Array2i &cell_index) const {
    return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x();
}

}  // namespace mapping
}  // namespace cartographer
